#include "board_com.h"
#include "can_common.h"
#include "message.h"
#include "offline.h"
#include "robotdef.h"
#include "rttypes.h"
#include "sbus.h"
#include "user_lib.h"
#include "referee.h"
#include "vcom.h"


#define LOG_TAG              "robotcontrol"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>


//部分仅限内部使用函数声明
static float CalcOffsetAngle(float getyawangle);
static void RemoteControlSet(Chassis_Ctrl_Cmd_s *Chassis_Ctrl,Shoot_Ctrl_Cmd_s *Shoot_Ctrl,Gimbal_Ctrl_Cmd_s *Gimbal_Ctrl);

#ifdef ONE_BOARD
    static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
    static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

    static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
    static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

    static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
    static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

    static Publisher_t *chassis_cmd_pub;              // 底盘控制消息发布者
    static Chassis_Ctrl_Cmd_s chassis_cmd_send;      // 发送给底盘应用的信息,包括控制信息和UI绘制相关

    static Subscriber_t *chassis_feed_sub;          // 底盘反馈信息订阅者
    static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等

    void robot_control_init()
    {
        gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
        gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

        shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

        chassis_cmd_pub= PubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
        chassis_feed_sub = SubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
    }

    void robot_control_task(void *paramter)
    {
        SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
        SubGetMessage(chassis_feed_sub, &chassis_fetch_data);
        Chassis_Ctrl_Cmd.offset_angle = CalcOffsetAngle(gimbal_fetch_data.yaw_motor_single_round_angle);
        RemoteControlSet();


        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
        PubPushMessage(chassis_cmd_pub, (void *)&chassis_cmd_send);
    }
#endif

#ifndef ONE_BOARD
    #if defined (GIMBAL_BOARD)
            static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
            static Gimbal_Ctrl_Cmd_s gimbal_cmd_send={0};      // 传递给云台的控制信息

            static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
            static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

            static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
            static Shoot_Ctrl_Cmd_s shoot_cmd_send={0};      // 传递给发射的控制信息

            static Chassis_Ctrl_Cmd_s chassis_cmd_send = {0};      // 发送给底盘应用的信息,包括控制信息和UI绘制相关
            static Chassis_Upload_Data_s chassis_fetch_data; // 从底盘应用接收的反馈信息信息,底盘功率枪口热量与底盘运动状态等
            void robot_control_init()
            {
                //订阅 发布注册
                gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
                gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
                shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));

                //板间通讯初始化
                board_com_init_t board_com_gimbal_init;
                board_com_gimbal_init.CanMessage.can_handle = &can2_bus;
                board_com_gimbal_init.offline_manage_init.event = OFFLINE_BOARD_CHASSIS;
                board_com_gimbal_init.offline_manage_init.error_level = OFFLINE_WARNING_LEVEL;
                board_com_gimbal_init.offline_manage_init.enable = OFFLINE_ENABLE;
                board_com_gimbal_init.offline_manage_init.beep_times = 6;
                board_com_gimbal_init.offline_manage_init.offline_time = 2000;
                board_com_init(board_com_gimbal_init);
            }

            void robot_control()
            {
                SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
                chassis_cmd_send.vx =0;
                chassis_cmd_send.vy =0;
                chassis_cmd_send.wz =0;
                chassis_cmd_send.offset_angle = 0;
                chassis_cmd_send.chassis_mode = CHASSIS_ZERO_FORCE;
                chassis_cmd_send.offset_angle = CalcOffsetAngle(gimbal_fetch_data.yaw_motor_single_round_angle);
                RemoteControlSet(&chassis_cmd_send,&shoot_cmd_send,&gimbal_cmd_send);
                
                PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
                PubPushMessage(shoot_cmd_pub,(void *)&shoot_cmd_send);

            
                if (gimbal_cmd_send.gimbal_mode == GIMBAL_AUTO_MODE) 
                {
                    float tmpvx = Nav_Recv.vx ;
                    float tmpvy = Nav_Recv.vy * (-1);
                    chassis_cmd_send.vx =tmpvx;
                    chassis_cmd_send.vy =tmpvy;
                    board_send(GIMBAL_ID,&chassis_cmd_send);    
                    offline_event_enable(OFFLINE_MINIPC_HEART);
                }
                else 
                {
                    offline_event_disable(OFFLINE_MINIPC_HEART);
                    board_send(GIMBAL_ID,&chassis_cmd_send);    
                }

                Nav_Send.header = 0x5A;
                Nav_Send.current_hp_percent = 400;
                Nav_Send.game_progress = 1;
                Nav_Send.event_center_aera_rmul = 0;
                Nav_Send.event_supply_area_rmul =0;
                Nav_Send.event_supply_area_rmuc = 0;
                Nav_Send.end = 0x5D ;
                nav_send_process(&Nav_Send);
            } 
    #else
            static Chassis_referee_Upload_Data_s Chassis_referee_Upload_Data;
            void robot_control_init()
            {
                //板间通讯初始化
                board_com_init_t board_com_gimbal_init;
                board_com_gimbal_init.CanMessage.can_handle = &can2_bus;
                board_com_gimbal_init.offline_manage_init.event = OFFLINE_BOARD_YAW;
                board_com_gimbal_init.offline_manage_init.error_level = OFFLINE_ERROR_LEVEL;
                board_com_gimbal_init.offline_manage_init.enable = OFFLINE_ENABLE;
                board_com_gimbal_init.offline_manage_init.beep_times = 6;
                board_com_gimbal_init.offline_manage_init.offline_time = 500;
                board_com_init(board_com_gimbal_init);
            }

            void robot_control()
            {
                board_send(CHASSIS_ID,&Chassis_referee_Upload_Data);     
            } 
    #endif 

    #if defined (DRONE_MODE)
        static Publisher_t *gimbal_cmd_pub;            // 云台控制消息发布者
        static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;      // 传递给云台的控制信息

        static Subscriber_t *gimbal_feed_sub;          // 云台反馈信息订阅者
        static Gimbal_Upload_Data_s gimbal_fetch_data; // 从云台获取的反馈信息

        static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
        static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息

        void robot_control_init()
        {
            gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
            gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

            shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
        }

        void robot_control()
        {
            CalcOffsetAngle();
            RemoteControlSet();
            SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);

            PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
            PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
        }
    #endif

#endif 


/**
 * @brief 根据gimbal app传回的当前电机角度计算和零位的误差
 *        单圈绝对角度的范围是0~360,说明文档中有图示
 *
 */
#if defined (GIMBAL_BOARD) || defined (ONE_BOARD) 
static float CalcOffsetAngle(float getyawangle)
{
    static float offsetangle;
    // 从云台获取的当前yaw电机单圈角度
#if YAW_ECD_GREATER_THAN_4096                               // 如果大于180度
    if (getyawangle > YAW_ALIGN_ANGLE && getyawangle <= 180.0f + YAW_ALIGN_ANGLE)
    {    
        offsetangle = getyawangle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else if (getyawangle > 180.0f + YAW_ALIGN_ANGLE)
    {    
        offsetangle = getyawangle - YAW_ALIGN_ANGLE - 360.0f;
        return offsetangle;
    }
    else
    {
        offsetangle = getyawangle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
#else // 小于180度
    if (getyawangle > YAW_ALIGN_ANGLE)
    {    
        offsetangle = getyawangle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else if (getyawangle <= YAW_ALIGN_ANGLE && getyawangle >= YAW_ALIGN_ANGLE - 180.0f)
    {
        offsetangle = getyawangle - YAW_ALIGN_ANGLE;
        return offsetangle;
    }
    else
    {
        offsetangle = getyawangle - YAW_ALIGN_ANGLE + 360.0f;
        return offsetangle;
    }
#endif
}


#define DEAD_ZONE(value, min, max, center) ((value) >= (min) && (value) <= (max) ? (center) : (value))

#if CONTROL_SOURCE == 1
static void RemoteControlSet(Chassis_Ctrl_Cmd_s *Chassis_Ctrl,Shoot_Ctrl_Cmd_s *Shoot_Ctrl,Gimbal_Ctrl_Cmd_s *Gimbal_Ctrl)
{
    if (offline_manage[OFFLINE_SBUS].online_state == STATE_ONLINE)
    {
        Chassis_Ctrl->vx = -790.0f * (float)(sbus.SBUS_CH.CH2 - SBUS_CHX_BIAS) / ((SBUS_CHX_UP - SBUS_CHX_DOWN) / 2.0f);
        Chassis_Ctrl->vy = 790.0f * (float)(sbus.SBUS_CH.CH1 - SBUS_CHX_BIAS) / ((SBUS_CHX_UP - SBUS_CHX_DOWN) / 2.0f); //根据 2 * PI * wheel_radius * rpm / 60.0f可得最大线速度

        switch (sbus.SBUS_CH.CH6)
        {
            case SBUS_CHX_DOWN:
                    Gimbal_Ctrl->gimbal_mode = GIMBAL_KEEPING_SMALL_YAW;

                    Gimbal_Ctrl->yaw -= 0.001f * (float)(sbus.SBUS_CH.CH4  - 1024);

                    Gimbal_Ctrl->small_yaw = SMALL_YAW_ALIGN_ANGLE;
                    Gimbal_Ctrl->pitch = SMALL_YAW_PITCH_HORIZON_ANGLE;
                    break;

            case SBUS_CHX_BIAS:
                    Gimbal_Ctrl->gimbal_mode = GIMBAL_KEEPING_BIG_YAW;
                    
                    Gimbal_Ctrl->small_yaw -= 0.001f * (float)(sbus.SBUS_CH.CH4  - 1024);
                    VAL_LIMIT(Gimbal_Ctrl->small_yaw, SMALL_YAW_MIN_ANGLE, SMALL_YAW_MAX_ANGLE);
                    Gimbal_Ctrl->pitch += 0.001f * (float)(sbus.SBUS_CH.CH3 - 1024);
                    VAL_LIMIT(Gimbal_Ctrl->pitch, SMALL_YAW_PITCH_MIN_ANGLE, SMALL_YAW_PITCH_MAX_ANGLE);
                    break;

            case SBUS_CHX_UP:
                    Gimbal_Ctrl->gimbal_mode = GIMBAL_AUTO_MODE;
                    Chassis_Ctrl->chassis_mode = CHASSIS_AUTO_MODE;
                    break;

            default:
                break;
        }

        // 处理CH9和CH10的逻辑
        if (sbus.SBUS_CH.CH9 > SBUS_CHX_BIAS)
        {
            
        }
        if (sbus.SBUS_CH.CH10 > SBUS_CHX_BIAS)
        {
            
        }

        // 处理射击控制逻辑
        switch (sbus.SBUS_CH.CH5)
        {
            case SBUS_CHX_DOWN:
            Shoot_Ctrl->shoot_mode =SHOOT_OFF;
                Shoot_Ctrl->friction_mode = FRICTION_OFF;
                Shoot_Ctrl->load_mode = LOAD_STOP;
                break;

            case SBUS_CHX_BIAS:
                Shoot_Ctrl->shoot_mode =SHOOT_ON;
                Shoot_Ctrl->friction_mode = FRICTION_OFF;
                Shoot_Ctrl->load_mode = LOAD_STOP;
                break;

            case SBUS_CHX_UP:
                Shoot_Ctrl->friction_mode = FRICTION_ON;
                if (sbus.SBUS_CH.CH7 == SBUS_CHX_DOWN)
                {
                    Shoot_Ctrl->load_mode = LOAD_1_BULLET;
                }
                else if (sbus.SBUS_CH.CH7 == SBUS_CHX_BIAS)
                {
                    Shoot_Ctrl->load_mode = LOAD_STOP;
                }
                else if (sbus.SBUS_CH.CH7 == SBUS_CHX_UP)
                {
                    Shoot_Ctrl->load_mode = LOAD_BURSTFIRE;
                }
                else
                {
                    Shoot_Ctrl->load_mode = LOAD_STOP;
                }
                break;

            default:
                break;
        }

        // 处理底盘控制逻辑
        switch (sbus.SBUS_CH.CH8)
        {
            case SBUS_CHX_DOWN:
                if(Chassis_Ctrl->chassis_mode != CHASSIS_AUTO_MODE){
                    Chassis_Ctrl->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
                }
                break;

            case SBUS_CHX_BIAS:
                Chassis_Ctrl->chassis_mode = CHASSIS_ROTATE;
                break;

            case SBUS_CHX_UP:
                Chassis_Ctrl->chassis_mode = CHASSIS_ROTATE_REVERSE;
                break;

            default:
                break;
        }
    }
    else
    {
        // 处理离线状态
        Gimbal_Ctrl->gimbal_mode = GIMBAL_ZERO_FORCE;
        Chassis_Ctrl->chassis_mode = CHASSIS_ZERO_FORCE;
        Shoot_Ctrl->shoot_mode = SHOOT_OFF;
        Shoot_Ctrl->friction_mode = FRICTION_OFF;
        Shoot_Ctrl->load_mode = LOAD_STOP;
    }
}
#endif

#if CONTROL_SOURCE == 3
static void RemoteControlSet(Chassis_Ctrl_Cmd_s *Chassis_Ctrl, Shoot_Ctrl_Cmd_s *Shoot_Ctrl, Gimbal_Ctrl_Cmd_s *Gimbal_Ctrl)
{
    static uint8_t last_fn1_pressed = 0;
    static uint8_t last_fn2_pressed = 0;
    static uint8_t last_trigger_pressed = 0;
    static uint8_t last_pause_pressed = 0;

    if (offline_manage[OFFLINE_REFEREE_SYSTEM].online_state == STATE_ONLINE)
    {
        // 处理底盘控制逻辑
        Chassis_Ctrl->vx = -790.0f * (float)(referee_remote_data.ch_1 - 1024) / ((1684 - 364) / 2.0f);
        Chassis_Ctrl->vy = 790.0f * (float)(referee_remote_data.ch_0 - 1024) / ((1684 - 364) / 2.0f);
        //Chassis_Ctrl->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;

        // 处理云台控制逻辑
        switch (referee_remote_data.mode_sw)
        {
            case 0:
                Gimbal_Ctrl->gimbal_mode = GIMBAL_KEEPING_SMALL_YAW;

                Gimbal_Ctrl->yaw -= 0.001f * (float)(referee_remote_data.ch_3 - 1024);
                Gimbal_Ctrl->small_yaw = SMALL_YAW_ALIGN_ANGLE;
                Gimbal_Ctrl->pitch = SMALL_YAW_PITCH_HORIZON_ANGLE;
                break;
            case 1:
                Gimbal_Ctrl->gimbal_mode = GIMBAL_KEEPING_BIG_YAW;
                
                Gimbal_Ctrl->small_yaw -= 0.001f * (float)(referee_remote_data.ch_3 - 1024);
                VAL_LIMIT(Gimbal_Ctrl->small_yaw, SMALL_YAW_MIN_ANGLE, SMALL_YAW_MAX_ANGLE);
                Gimbal_Ctrl->pitch += 0.001f * (float)(referee_remote_data.ch_2 - 1024);
                VAL_LIMIT(Gimbal_Ctrl->pitch, SMALL_YAW_PITCH_MIN_ANGLE, SMALL_YAW_PITCH_MAX_ANGLE);
                break;
            case 2:
                Gimbal_Ctrl->gimbal_mode = GIMBAL_AUTO_MODE;
                break;
            default:
                break;
        } 

        // 处理射击控制逻辑
        if (referee_remote_data.trigger == 1 && last_trigger_pressed == 0)
        {
            if (Shoot_Ctrl->friction_mode == FRICTION_ON)
            {
                Shoot_Ctrl->shoot_mode = SHOOT_ON;
                Shoot_Ctrl->friction_mode = FRICTION_OFF;
                Shoot_Ctrl->load_mode = LOAD_STOP;
            }
            else
            {
                Shoot_Ctrl->shoot_mode = SHOOT_ON;
                Shoot_Ctrl->friction_mode = FRICTION_ON;
            }
        }
        last_trigger_pressed = referee_remote_data.trigger;

        if (Shoot_Ctrl->friction_mode == FRICTION_ON)
        {
            switch (referee_remote_data.wheel) 
            {
                case 364:
                    Shoot_Ctrl->load_mode = LOAD_1_BULLET;
                    break;
                case 1024:
                    Shoot_Ctrl->load_mode = LOAD_STOP;
                    break;
                case 1684:
                default:
                    Shoot_Ctrl->load_mode = LOAD_BURSTFIRE;
                    break;
            }
        }
        else
        {
            Shoot_Ctrl->load_mode = LOAD_STOP;
        }

        // 处理底盘控制逻辑
        if (referee_remote_data.fn_1 == 1 && last_fn1_pressed == 0) 
        {
            if (Chassis_Ctrl->chassis_mode == CHASSIS_FOLLOW_GIMBAL_YAW) {Chassis_Ctrl->chassis_mode = CHASSIS_ROTATE;} 
            else {Chassis_Ctrl->chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;}
        }
        last_fn1_pressed = referee_remote_data.fn_1;

        // 仅在 ROTATE 模式下处理 fn_2 切换旋转方向
        if (Chassis_Ctrl->chassis_mode == CHASSIS_ROTATE || Chassis_Ctrl->chassis_mode == CHASSIS_ROTATE_REVERSE) {
            if (referee_remote_data.fn_2 == 1 && last_fn2_pressed == 0) {
                if (Chassis_Ctrl->chassis_mode == CHASSIS_ROTATE) {Chassis_Ctrl->chassis_mode = CHASSIS_ROTATE_REVERSE;} 
                else {
                    Chassis_Ctrl->chassis_mode = CHASSIS_ROTATE;
                }
            }
            last_fn2_pressed = referee_remote_data.fn_2;
        }
    }
    else
    {
        // 处理离线状态
        Gimbal_Ctrl->gimbal_mode = GIMBAL_ZERO_FORCE;
        Chassis_Ctrl->chassis_mode = CHASSIS_ZERO_FORCE;
        Shoot_Ctrl->shoot_mode = SHOOT_OFF;
        Shoot_Ctrl->friction_mode = FRICTION_OFF;
        Shoot_Ctrl->load_mode = LOAD_STOP;
    }
}
#endif

#endif